#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include "std_msgs/msg/float32.hpp"
#include "obstacle_position.h"
#include "desired_input_v2.h"
#include "observed_tar_input_v2.h"
#include "rfnn_fcn_v3.h"
#include "rfnn_to_vel.h"
#include "desired_vel_vg.h"
#include "jac_obt_vR.h"
#include "jac_obt_vR_initialize.h"
#include "EKF_v5g.h"
#include "EKF_v5g_initialize.h"
#include "desired_vel_v2.h"
#include <mutex>
#include <fstream>

//在工作空间建立文件weight_change.txt
//在工作空间建立文件alg_data.txt (dg,dob,sig,sio,dgd,dobd,sigd,siod,vL,vR,vLd,vRd)

// float d=0.07,b=0.098;
float d = 0.085, b = 0.128;
float PI=M_PI;

float ekf_pr;

double aph=2.4, beta=4;

double tar[2]={8,1};
double dg, sig, dob=4.0, sio=0.0;
double dg1,dob1,sig1,sio1;
double dgd, sigd, dobd=4.0, siod=0.0;
double x,y,car_ang,xd,yd,car_ang_d,R2,R5,R9;
double xob,yob;

double vL=0.0,vR=0.0,vLd=0.0,vRd=0.0;

// double a[32]={0.11,0.14,0.17,0.62,0.57,0.05,0.93,0.73,0.74,0.06,0.86,0.93,0.98,0.86,0.79,0.51,
//     0.18,0.40,0.13,0.03,0.94,0.30,0.30,0.33,0.47,0.65,0.03,0.84,0.56,0.85,0.35,0.45};//原权重
double a[32]={0.95,0.28,0.24,0.19,0.92,0.14,1.09,0.89,0.97,0.1,0.87,1.02,0.92,0.7,0.84,0.63,
    0.9,0.46,0.2,0.73,1.4,0.44,0.45,0.41,0.67,0.75,0.04,1.12,0.69,1.12,0.38,0.52};//train后的权重
double Fnk[16];
double Outputs[2];
int k=0;
double Fnk_pre[16];
double S_pre,Outputs_pre[2],car_xy[2],obs_p[2],pre_mat[3];
double dg_a[32],do_a[32],sig_a[32],sio_a[32];
double aOUT[32];

class RfnnEkfMov4 : public rclcpp::Node
{
private:
    std::ofstream data_file_;  // 用于写入数据的文件流对象
    std::ofstream data_file2_;  // 用于写入数据的文件流对象
    std::mutex mutex_;
    //声明lidar_dis_angle主题
    rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr laser_scan_sub_;
    void lidar_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
    {
        // std::lock_guard<std::mutex> lock(mutex_);
        // RCLCPP_INFO(this->get_logger(),"以接收最短距离=%f,对应角度=%f",msg->data[0],msg->data[1]);
        dob=msg->data[0];
        sio=msg->data[1];
    }
    //声明pos_obs_des
    rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr position_sub_;
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr command_publish_vel;
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr command_publish_data;
    // 声明话题发布者L
    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr command_publisherL_;
    // 声明话题发布者R
    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr command_publisherR_;
    void rfnn_ekf_fcn(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
    {
        // std::lock_guard<std::mutex> lock(mutex_);
        //观测值
        x=static_cast<double>(msg->data[0]);
        y=static_cast<double>(msg->data[1]);
        car_ang=static_cast<double>(msg->data[2]);
        //期望值
        xd=static_cast<double>(msg->data[3]);
        yd=static_cast<double>(msg->data[4]);
        car_ang_d=static_cast<double>(msg->data[5]);
        //矩阵
        R2=static_cast<double>(msg->data[6]);
        R5=static_cast<double>(msg->data[7]);
        R9=static_cast<double>(msg->data[8]);
        //ekf运行许可
        ekf_pr=msg->data[9];
        /**障碍物信息************************************************************************************/
        //
        obstacle_position(dob, sio, x, y,
        car_ang, &xob, &yob);
        RCLCPP_INFO(this->get_logger(),"以接收最短距离=%f,对应角度=%f",dob,sio);
        RCLCPP_INFO(this->get_logger(), "障碍物位置 xob: %f, yob: %f", xob, yob);
        //
        /***rfnn期望和实际输入信息**********************************************************************/
        //
        desired_input_v2(xd, yd, car_ang_d, tar[0],
            tar[1], xob, yob, &dgd, &dobd,
            &sigd, &siod);
        RCLCPP_INFO(this->get_logger(), "期望rfnn dgd: %f, dobd: %f, sigd: %f, siod: %f", dgd, dobd,sigd,siod);
        observed_tar_input_v2(x, y, car_ang, tar[0],
                    tar[1], &dg, &sig);
        RCLCPP_INFO(this->get_logger(), "观测rfnn dg: %f, dob: %f, sig: %f, sio: %f", dg, dob,sig,sio);
        std_msgs::msg::Float32MultiArray in_rfnn_data;
        in_rfnn_data.data.resize(4);
        in_rfnn_data.data[0]=(float)dg;
        in_rfnn_data.data[1]=(float)sig;
        in_rfnn_data.data[2]=(float)dob;
        in_rfnn_data.data[3]=(float)sio;
        // 将更新后的a[i]写入文件，后面跟一个空格分隔
        data_file2_ << dg << ","<<dob<<","<<sig<<","<<sio<<","<<dgd<<","<<dobd<<","<<sigd<<","<<siod<<",";
        command_publish_data->publish(in_rfnn_data);
        //
        /*rfnn处理**********************************************************************************/
        if (dg>0.8)
        {
            // rfnn_fcn_v3(a,b,dg,sig,dob,sio,&vR,&vL,Fnk,Outputs);
            /**RFNN输入值设置阈值*****************************/
            if(dg>10.0)
            {
                dg1=10.0;
            }
            else
            {
                dg1=dg;
            }
            if (dob>3.0)
            {
                dob1=3;
            }
            else
            {
                dob1=dob;
            }
            if (sig1>PI/2.0)
            {
                sig1=PI/2.0;
            }
            else if (sig1<-PI/2.0)
            {
                sig1=-PI/2.0;
            }
            else
            {
                sig1=sig;
            }
            if (sio1>PI/2.0)
            {
                sio1=PI/2.0;
            }
            else if (sio1<-PI/2.0)
            {
                sio1=-PI/2.0;
            }
            else
            {
                sio1=sio;
            }
            /*****************************************************/
            rfnn_fcn_v3(a,dg1,sig1,dob1,sio1,Fnk,Outputs);
            rfnn_to_vel(Outputs,b,aph,beta,&vR,&vL);
            // desired_vel_vg(dg1,dob1,sig1,sio1,b,&vRd,&vLd);
            desired_vel_v2(dg1,dob1,sig1,sio1,beta,aph,b,&vRd,&vLd);
            // vRd=vR;
            // vLd=vL;
            // RCLCPP_INFO(this->get_logger(), "权重: a=%f", a);
        }
        else
        {
            vR=0.0;
            vL=0.0;
            vRd=0.0;
            vLd=0.0;
        }
        data_file2_ <<vL<<","<<vR<<","<<vLd<<","<<vRd<<",";
        data_file2_ << std::endl;
        //
        /*发布速度**********************************************************************************/
        //
        std_msgs::msg::Float32 vL_Setpoint1;
        std_msgs::msg::Float32 vR_Setpoint2;
        std_msgs::msg::Float32MultiArray vel_data;
        vel_data.data.resize(4);
        vel_data.data[0]=(float)vR;
        vel_data.data[1]=(float)vL;
        vel_data.data[2]=(float)vRd;
        vel_data.data[3]=(float)vLd;
        RCLCPP_INFO(this->get_logger(), "期望速度: vRd=%f, vLd=%f", vRd,vLd);
        RCLCPP_INFO(this->get_logger(), "rfnn速度: vR=%f, vL=%f", vR,vL);
        //
        vL_Setpoint1.data=((float)vL)/(d/2.0)/(2*PI)*60.0;
        vR_Setpoint2.data=((float)vR)/(d/2.0)/(2*PI)*60.0;
        command_publisherL_->publish(vL_Setpoint1);
        command_publisherR_->publish(vR_Setpoint2);
        command_publish_vel->publish(vel_data);
        //
        /*efk调整********************************************************************************************/
        //
        if (dg>0.8)
        {
            if (k==1)
            {
                car_xy[0]=x;
                car_xy[1]=y;
                obs_p[0]=xob;
                obs_p[1]=yob;
                jac_obt_vR(a,Fnk_pre,S_pre,Outputs_pre,aph,b,tar,car_xy,obs_p,pre_mat,dg_a,do_a,sig_a,sio_a);
                if(ekf_pr==1.0)
                {
                    EKF_v5g(dg_a,do_a,sig_a,sio_a,dg,dob,sig,sio,a,dgd,dobd,sigd,siod,aOUT);
                    for (size_t i = 0; i < 32; i++)
                    {
                        if(aOUT[i]>0.01)
                        {
                            a[i]=aOUT[i];
                        }
                        // 将更新后的a[i]写入文件，后面跟一个空格分隔
                        data_file_ << a[i] << ",";
                    }
                    // 32个数字都输出完后，写入一个换行符
                    data_file_ << std::endl;
                }
            }
            k=1;
            pre_mat[0]=R2;
            pre_mat[1]=R5;
            pre_mat[2]=R9;
            for (size_t i = 0; i <16; i++)
            {
                Fnk_pre[i]=Fnk[i];
            }
            for (size_t i = 0; i < 2; i++)
            {
                Outputs_pre[i]=Outputs[i];
            }
            S_pre=dg1+dob1+sig1+sio1;
        }
        
    }
public:
    RfnnEkfMov4(std::string name) : Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());
        // 打开文件用于写入，文件名为"data_output.txt"
        data_file_.open("weight_change.txt", std::ios::out | std::ios::trunc);// 使用trunc模式清空文件内容
        data_file2_.open("alg_data.txt", std::ios::out | std::ios::trunc);
        if (!data_file_.is_open())
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to open file for writing.");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "File opened successfully for writing data.");
        }
        if (!data_file2_.is_open())
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to open file2 for writing.");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "File2 opened successfully for writing data.");
        }
        // 订阅pos_obs_des主题
        position_sub_=this->create_subscription<std_msgs::msg::Float32MultiArray>(
            "pos_obs_des", 10, 
            std::bind(&RfnnEkfMov4::rfnn_ekf_fcn, this, 
            std::placeholders::_1));
        //发布速度
        // 创建发布者L
        command_publisherL_=this->create_publisher<std_msgs::msg::Float32>("cal_vel_l", 10);
        // 创建发布者R
        command_publisherR_=this->create_publisher<std_msgs::msg::Float32>("cal_vel_r", 10);
        //创建发布速度数组
        command_publish_vel=this->create_publisher<std_msgs::msg::Float32MultiArray>("vel_g", 10);
        // 订阅激光雷达lidar_dis_angle主题
        laser_scan_sub_=this->create_subscription<std_msgs::msg::Float32MultiArray>(
            "lidar_dis_angle", 10, 
            std::bind(&RfnnEkfMov4::lidar_callback, this, 
            std::placeholders::_1));
         //创建发布rfnn input
        command_publish_data=this->create_publisher<std_msgs::msg::Float32MultiArray>("in_data", 10);
    }
    ~RfnnEkfMov4();  // 析构函数声明
};

RfnnEkfMov4::~RfnnEkfMov4()
{
    if (data_file_.is_open())
    {
        data_file_.close();
        RCLCPP_INFO(this->get_logger(), "File closed successfully.");
    }
    if (data_file2_.is_open())
    {
        data_file2_.close();
        RCLCPP_INFO(this->get_logger(), "File2 closed successfully.");
    }
}

int main(int argc, char **argv)
{
    static bool initialized = false;
    if (!initialized) {
        jac_obt_vR_initialize();
        EKF_v5g_initialize();
        initialized = true;
    }
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<RfnnEkfMov4>("rfnn_ekf_mov4");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}